// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PathPlanner.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Motion
{
    using System;
    using System.Collections.Generic;
    using System.Diagnostics;
    using System.Linq;
    using System.Runtime.CompilerServices;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// Path planner.
    /// </summary>
    /// <remarks>
    /// This planner generates straightened paths very quickly. The goal has been to produce
    /// a plans on a megapixel map at 30 hertz. Conceptually, these are the stages:
    ///   - Generate Chamfer distance map with origins (~19ms)
    ///   - Generate corridor map from distance threshold (~5ms)
    ///   - Generate skeleton from ridges in distance map (~9ms)
    ///   - Join current/goal points to skeleton
    ///   - A* search for path within skeleton (~8ms)
    ///   - Straighten path by line-of-sight within corridors (~3ms)
    /// In reality, the generation of a distance map with origins, the corridor map, and the
    /// skeleton are all done in the same pass; resulting in ~22ms total. Combined with the
    /// A* search and straightening, we come in at *just* 30 hertz.
    /// </remarks>
    [DataContract]
    public sealed class PathPlanner : IGridBasedPathPlanner
    {
        /// <summary>
        /// Distance map to nearest obstacles.
        /// </summary>
        private int[] distanceMap;

        /// <summary>
        /// Corridor map within open space.
        /// </summary>
        private bool[] corridorMap;

        /// <summary>
        /// Skeleton map of corridor paths.
        /// </summary>
        private bool[] skeletonMap;

        /// <summary>
        /// Open cells (used during Chamfer distance calculation).
        /// </summary>
        private bool[] open;

        /// <summary>
        /// X origin of Chamfer distance calculation.
        /// </summary>
        private int[] originX;

        /// <summary>
        /// Y origin of Chamfer distance calculation.
        /// </summary>
        private int[] originY;

        /// <summary>
        /// Close points during search.
        /// </summary>
        private bool[] close;

        /// <summary>
        /// Used to mark cells during search.
        /// </summary>
        private bool[] mark;

        /// <summary>
        /// Cells retracing steps taken during search.
        /// </summary>
        /// <remarks>
        /// Used during the search to record where each cell in the "frontier" came
        /// from; to "retrace" the steps to reconstruct the path once one of the
        /// frontier cells ends up being the goal and we're done.
        /// </remarks>
        private int[] retrace;

        /// <summary>
        /// Score during search.
        /// </summary>
        [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.StyleCop.CSharp.NamingRules", "SA1305:FieldNamesMustNotUseHungarianNotation", Justification = "Standard A* naming for fScore/gScore.")]
        private int[] gScore;

        /// <summary>
        /// Score during search.
        /// </summary>
        [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.StyleCop.CSharp.NamingRules", "SA1305:FieldNamesMustNotUseHungarianNotation", Justification = "Standard A* naming for fScore/gScore.")]
        private int[] fScore;

        /// <summary>
        /// Width of the map in grid squares.
        /// </summary>
        private int mapWidth;

        /// <summary>
        /// Height of the map in grid squares.
        /// </summary>
        private int mapHeight;

        /// <summary>
        /// Radius of the robot in grid squares.
        /// </summary>
        private int robotRadius;

        /// <summary>, 
        /// Buffer space away from obstacles to remain "comfortable".
        /// </summary>
        /// <remarks>
        /// Straightened plans will avoid violating this buffer except when necessary
        /// (for example to pass through a door). In other words, straightened paths
        /// revert to following the skeleton tightly (non-straightened form) when the
        /// corridor is blocked.
        /// </remarks>
        private int corridorBuffer;

        /// <summary>
        /// Skeleton map threshold used to weed out small ridges.
        /// </summary>
        private int skeletonThreshold;

        /// <summary>
        /// Last seen global map.
        /// </summary>
        private int mapHash;

        /// <summary>
        /// Initializes the path planner
        /// </summary>
        /// <param name="width">Width of map in grid squares.</param>
        /// <param name="height">Height of map in grid squares.</param>
        /// <param name="corridorBuffer">Distance from walls and obstacles considered open for straight paths.</param>
        /// <param name="skeletonThreshold">Maximum distance between origin between adjacent points along a ridge, to be considered part of the skeleton.</param>
        /// <param name="radius">Radius of the robot in grid squares</param>
        public void Initialize(int width, int height, int corridorBuffer, int skeletonThreshold, int radius = 0)
        {
            if (width <= 0 || height <= 0)
            {
                throw new ArgumentException("'width' and 'height' must be greater than 0");
            }

            if (radius < 0)
            {
                throw new ArgumentException("'radius' must be non-negative");
            }

            this.mapHeight = height;
            this.mapWidth = width;
            this.robotRadius = radius;
            this.corridorBuffer = corridorBuffer;
            this.skeletonThreshold = skeletonThreshold;
        }

        /// <summary>
        /// Returns a path for the given map, and current/goal locations.
        /// </summary>
        /// <param name="map">Global map.</param>
        /// <param name="currentLocation">Current location.</param>
        /// <param name="goalLocation">Goal location.</param>
        /// <returns>An ordered list of the points from the current location to the goal location.</returns>
        public IEnumerable<Point2D<int>> GetPath(bool[] map, Point2D<int> currentLocation, Point2D<int> goalLocation)
        {
            var watch = new Stopwatch();
            watch.Start();

            if (map.Length != this.mapWidth * this.mapHeight)
            {
                throw new ArgumentException("Invalid map size for width and height");
            }

            if (
                map[PathPlanningCommon.PointToArrayIndex(currentLocation, this.mapWidth, this.mapHeight)] ||
                map[PathPlanningCommon.PointToArrayIndex(goalLocation, this.mapWidth, this.mapHeight)])
            {
                // current or goal point not within free space.
                TraceOut.Info(TraceContexts.Nav, "Current or goal point not within free space.");
                return new List<Point2D<int>>();
            }

            var len = this.mapWidth * this.mapHeight;
            if (this.distanceMap == null || this.distanceMap.Length != len)
            {
                // to save allocations, these are reused if possible
                this.distanceMap = new int[len];
                this.corridorMap = new bool[len];
                this.skeletonMap = new bool[len];
                this.originX = new int[len];
                this.originY = new int[len];
                this.open = new bool[len];
                this.close = new bool[len];
                this.mark = new bool[len];
                this.retrace = new int[len];
                this.gScore = new int[len];
                this.fScore = new int[len];
            }

            int mapHash;
            if (this.MapHasChanged(map, out mapHash))
            {
                this.mapHash = mapHash;
                this.BuildDistanceCorridorAndSkeletonMaps(map);
            }

            var joinedSkeletonMap = this.skeletonMap.Clone() as bool[]; // cloned to mutate
            this.JoinPointToSkeleton(currentLocation, joinedSkeletonMap);
            this.JoinPointToSkeleton(goalLocation, joinedSkeletonMap);

            var path = this.Search(currentLocation, goalLocation, joinedSkeletonMap);
            var straight = this.Straighten(path);

            watch.Stop();
            TraceOut.Info(TraceContexts.Nav, "Path plan time: {0}ms", watch.ElapsedMilliseconds);

            return straight;
        }

        /// <summary>
        /// Determine whether the map has changed based on hash.
        /// </summary>
        /// <param name="map">Global map.</param>
        /// <param name="mapHash">Map hash out param.</param>
        /// <returns>Whether the map has changed.</returns>
        internal bool MapHasChanged(bool[] map, out int mapHash)
        {
            mapHash = map.Aggregate(this.robotRadius, (x, y) => x.GetHashCode() ^ y.GetHashCode());
            return mapHash != this.mapHash;
        }

        /// <summary>
        /// ManhattanDistance between two points.
        /// </summary>
        /// <param name="px">X coordinate of first point.</param>
        /// <param name="py">Y coordinate of first point.</param>
        /// <param name="qx">X coordinate of second point.</param>
        /// <param name="qy">Y coordinate of second point.</param>
        /// <returns>Manhattan distance.</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static int ManhattanDistance(int px, int py, int qx, int qy)
        {
            var dx = px - qx;
            var dy = py - qy;
            return (dx < 0 ? -dx : dx) + (dy < 0 ? -dy : dy);
        }

        /// <summary>
        /// Apply function to each point along a line.
        /// </summary>
        /// <param name="x">X coordinate.</param>
        /// <param name="y">Y coordinate.</param>
        /// <param name="heading">Heading in radians.</param>
        /// <param name="length">Length of line.</param>
        /// <param name="minStep">Minimum step size.</param>
        /// <param name="fn">Function to be applied.</param>
        private static void ForEachPointInLine(int x, int y, double heading, int length, int minStep, Func<int, int, bool> fn)
        {
            // Bresenham's line algorithm (with ordering maintained)
            var x0 = x;
            var y0 = y;
            var x1 = x + (int)(Math.Sin(heading) * length + 0.5);
            var y1 = y + (int)(Math.Cos(heading) * length + 0.5);
            var steep = Math.Abs(y1 - y0) > Math.Abs(x1 - x0);
            if (steep)
            {
                var t = x0;
                x0 = y0;
                y0 = t;
                t = x1;
                x1 = y1;
                y1 = t;
            }

            var dx = Math.Abs(x1 - x0);
            var dy = Math.Abs(y1 - y0);
            var error = dx / 2;
            var xstep = x0 < x1 ? minStep : -minStep;
            var ystep = y0 < y1 ? minStep : -minStep;
            var j = y0;
            x1 = ((x1 - x0) / minStep) * minStep + x0;
            if (steep)
            {
                for (var i = x0; i != x1; i += xstep)
                {
                    if (fn(j, i))
                    {
                        return;
                    }

                    error -= dy;
                    if (error < 0)
                    {
                        j += ystep;
                        error += dx;
                    }
                }
            }
            else
            {
                for (var i = x0; i != x1; i += xstep)
                {
                    if (fn(i, j))
                    {
                        return;
                    }

                    error -= dy;
                    if (error < 0)
                    {
                        j += ystep;
                        error += dx;
                    }
                }
            }
        }

        /// <summary>
        /// Euclidean distance between two points.
        /// </summary>
        /// <param name="a">First point.</param>
        /// <param name="b">Second point.</param>
        /// <returns>Euclidean distance.</returns>
        private static double EuclideanDistance(Point2D<int> a, Point2D<int> b)
        {
            var x = a.X - b.X;
            var y = a.Y - b.Y;
            return Math.Sqrt(x * x + y * y);
        }

        /// <summary>
        /// Get eight neighbor points.
        /// </summary>
        /// <param name="i">Point index of which to get neighbors.</param>
        /// <returns>Array of neighbors.</returns>
        private int[] EightNeighbors(int i)
        {
            var imw = i - this.mapWidth;
            var ipw = i + this.mapWidth;

            return new int[]
            {
                imw - 1,
                imw,
                imw + 1,
                i - 1,
                i + 1,
                ipw - 1,
                ipw,
                ipw + 1
            };
        }

        /// <summary>
        /// Join point to skeleton by following distance gradient.
        /// </summary>
        /// <param name="point">Point to join to skeleton.</param>
        /// <param name="skeleton">Skeleton to which to join (mutated in place).</param>
        private void JoinPointToSkeleton(Point2D<int> point, bool[] skeleton)
        {
            var m = point.Y + point.X * this.mapWidth;
            var dm = this.distanceMap[m];
            while (!skeleton[m])
            {
                skeleton[m] = true;
                foreach (var n in this.EightNeighbors(m))
                {
                    if (n >= 0 && n < this.distanceMap.Length && this.distanceMap[n] > dm)
                    {
                        m = n;
                        dm = this.distanceMap[m];
                    }
                }
            }
        }

        /// <summary>
        /// Build distance, corridor and skeleton maps.
        /// </summary>
        /// <remarks>
        /// Here we do build several maps later used to compute and smooth the path.
        /// Essentially, this is a Chamfer distance map, but we keep track of the
        /// origin points for each cell (the nearest obstacle). Finding the 'corridor'
        /// is a simple threshold; leaving a buffer along the walls and such. Finding
        /// the skeleton is a matter of finding the ridges in the distance map. This
        /// is very ambiguous though and it's surprising that there aren't more published
        /// techniques for this! We use the pretty simple idea of finding neighboring
        /// cells whose origin points are greater than a threshold distance apart. That
        /// is, coming from opposite walls in a hallway for example.
        /// Ultimately, the below builds all of these maps in just two passes over the
        /// occupancy map given.
        /// </remarks>
        /// <param name="map">Occupancy map.</param>
        private void BuildDistanceCorridorAndSkeletonMaps(bool[] map)
        {
            // reinitialize
            for (var j = 0; j < map.Length; ++j)
            {
                this.distanceMap[j] = 0;
                this.corridorMap[j] = false;
                this.skeletonMap[j] = false;
                this.originX[j] = 0;
                this.originY[j] = 0;
                this.open[j] = false;
            }

            const int MaxDistance = int.MaxValue;

            // border max distance
            var pastEnd = this.mapWidth * this.mapHeight;
            var startOfLastRow = pastEnd - this.mapWidth;
            var endOfLastRow = pastEnd - 1;
            var startOfSecondRow = this.mapWidth;
            var endOfSecondRow = this.mapWidth * 2 - 1;

            var top = 0;
            var bottom = this.mapHeight - 1;
            var left = 0;
            var right = this.mapWidth - 1;

            // top/bottom
            for (int x1 = left, x2 = startOfLastRow; x1 <= right; ++x1, ++x2)
            {
                this.distanceMap[x1] = this.distanceMap[x2] = MaxDistance;
                this.originX[x1] = x1;
                this.originY[x1] = top;
                this.originX[x2] = x1;
                this.originY[x2] = bottom;
            }

            // left/right
            for (int y = 1, y1 = startOfSecondRow, y2 = endOfSecondRow; y1 < startOfLastRow; ++y, y1 += this.mapWidth, y2 += this.mapWidth)
            {
                this.distanceMap[y1] = this.distanceMap[y2] = MaxDistance;
                this.originX[y1] = left;
                this.originX[y1] = y;
                this.originY[y2] = right;
                this.originY[y2] = y;
            }

            var start = startOfSecondRow + 1;
            var end = endOfLastRow - this.mapWidth;

            // first pass
            var i = startOfSecondRow;
            for (var y = 1; y < this.mapHeight - 1; ++y, i += 2)
            {
                for (var x = 1; x < this.mapWidth - 1; ++x, ++i)
                {
                    if (map[i])
                    {
                        this.distanceMap[i] = 0;
                        this.originX[i] = x;
                        this.originY[i] = y;
                    }
                    else
                    {
                        this.open[i] = true;
                        var w = i - 1; // west
                        var n = i - this.mapWidth; // north
                        var dw = this.distanceMap[w];
                        var dn = this.distanceMap[n];
                        if (dn < dw)
                        {
                            this.distanceMap[i] = dn + 1;
                            this.originX[i] = this.originX[n];
                            this.originY[i] = this.originY[n];
                        }
                        else
                        {
                            this.distanceMap[i] = dw + 1;
                            this.originX[i] = this.originX[w];
                            this.originY[i] = this.originY[w];
                        }
                    }
                }
            }

            // second pass
            i -= 2; // second to end of end of second to last row
            for (var y = this.mapHeight - 2; y > 0; --y, i -= 2)
            {
                for (var x = this.mapWidth - 2; x > 0; --x, --i)
                {
                    if (this.open[i])
                    {
                        var e = i + 1; // east
                        var s = i + this.mapWidth; // south
                        var de = this.distanceMap[e];
                        var ds = this.distanceMap[s];
                        var di = this.distanceMap[i];
                        if (ds < de)
                        {
                            if (ds < di)
                            {
                                this.distanceMap[i] = di = ds + 1;
                                this.originX[i] = this.originX[s];
                                this.originY[i] = this.originY[s];
                                this.skeletonMap[i] = ManhattanDistance(this.originX[i], this.originY[i], this.originX[e], this.originY[e]) > this.skeletonThreshold;
                            }
                            else
                            {
                                var oix = this.originX[i];
                                var oiy = this.originY[i];
                                this.skeletonMap[i] =
                                    ManhattanDistance(oix, oiy, this.originX[s], this.originY[s]) > this.skeletonThreshold ||
                                    ManhattanDistance(oix, oiy, this.originX[e], this.originY[e]) > this.skeletonThreshold;
                            }
                        }
                        else
                        {
                            if (de < di)
                            {
                                this.distanceMap[i] = di = de + 1;
                                this.originX[i] = this.originX[e];
                                this.originY[i] = this.originY[e];
                                this.skeletonMap[i] = ManhattanDistance(this.originX[i], this.originY[i], this.originX[s], this.originY[s]) > this.skeletonThreshold;
                            }
                            else
                            {
                                var oix = this.originX[i];
                                var oiy = this.originY[i];
                                this.skeletonMap[i] =
                                    ManhattanDistance(oix, oiy, this.originX[s], this.originY[s]) > this.skeletonThreshold ||
                                    ManhattanDistance(oix, oiy, this.originX[e], this.originY[e]) > this.skeletonThreshold;
                            }
                        }

                        this.corridorMap[i] = (di > this.corridorBuffer);
                    }
                }
            }
        }

        /// <summary>
        /// A* search for a path from current to goal point.
        /// </summary>
        /// <param name="currentLocation">Current point.</param>
        /// <param name="goalLocation">Goal point.</param>
        /// <param name="skeleton">Skeleton map (with current and goal points joined).</param>
        /// <returns>List of points representing the path.</returns>
        private List<Point2D<int>> Search(Point2D<int> currentLocation, Point2D<int> goalLocation, bool[] skeleton)
        {
            var open = new SortedList<int, Queue<int>>();

            // reintialize
            for (var i = 0; i < this.corridorMap.Length; ++i)
            {
                this.close[i] = false;
                this.retrace[i] = 0;
                this.gScore[i] = -1;
                this.fScore[i] = 0;
                this.mark[i] = false;
            }

            var xy = PathPlanningCommon.PointToArrayIndex(currentLocation, this.mapWidth, this.mapHeight);
            open.Add(0, new Queue<int>(new int[] { xy }));
            this.mark[xy] = true;
            this.gScore[xy] = 0;
            this.fScore[xy] = this.gScore[xy] + ManhattanDistance(currentLocation.X, currentLocation.Y, goalLocation.X, goalLocation.Y);
            var gxy = goalLocation.Y + goalLocation.X * this.mapWidth;

            while (open.Count > 0)
            {
                var lowest = open.First();
                var cxy = lowest.Value.Dequeue();
                if (lowest.Value.Count == 0)
                {
                    open.Remove(lowest.Key);
                }

                if (cxy == gxy)
                {
                    // reconstruct path!
                    var plan = new List<Point2D<int>>();
                    var c = cxy;
                    while (c != xy)
                    {
                        plan.Add(new Point2D<int>(c / this.mapWidth, c % this.mapWidth));
                        c = this.retrace[c];
                    }

                    plan.Reverse();

                    return plan;
                }

                this.mark[cxy] = false;
                this.close[cxy] = true;

                foreach (var nxy in this.EightNeighbors(cxy))
                {
                    if (skeleton[nxy])
                    {
                        var x = nxy / this.mapWidth;
                        var y = nxy % this.mapWidth;

                        var tentGScore = this.gScore[cxy] + 1;
                        var tentFScore = tentGScore + ManhattanDistance(x, y, goalLocation.X, goalLocation.Y);
                        if (!(this.close[nxy] && tentFScore >= this.fScore[nxy]))
                        {
                            var notInOpenSet = !this.mark[nxy];
                            if (notInOpenSet || tentFScore < this.fScore[nxy])
                            {
                                this.retrace[nxy] = cxy;
                                this.gScore[nxy] = tentGScore;
                                this.fScore[nxy] = tentFScore;
                                if (notInOpenSet)
                                {
                                    var gs = this.fScore[nxy];
                                    if (!open.ContainsKey(gs))
                                    {
                                        open.Add(gs, new Queue<int>());
                                    }

                                    open[gs].Enqueue(nxy);
                                    this.mark[nxy] = true;
                                }
                            }
                        }
                    }
                }
            }

            return new List<Point2D<int>>();
        }

        /// <summary>
        /// Find the distance to the boundary of the corridor map from a given pose.
        /// </summary>
        /// <param name="x">X coordinate.</param>
        /// <param name="y">Y coordinate.</param>
        /// <param name="heading">Heading in radians.</param>
        /// <param name="min">Minimum distance (step size).</param>
        /// <returns>Distance to boundary.</returns>
        private int DistanceToBoundaryOfCorridor(int x, int y, double heading, int min)
        {
            var max = this.mapWidth + this.mapHeight; // max manhattan distance (quicker than Euclidean)
            int d = 0;
            ForEachPointInLine(
                x,
                y,
                heading,
                max,
                min,
                (i, j) =>
                {
                    d += min;
                    return (i < 0 || j < 0 || i >= this.mapHeight || j >= this.mapWidth || !this.corridorMap[j + i * this.mapWidth]);
                });

            return d;
        }

        /// <summary>
        /// Straighten path.
        /// </summary>
        /// <remarks>
        /// The path coming into this function follows the skeleton. This can be quite
        /// jagged. Here we straighten the path by essentially tracking toward a point
        /// as far ahead on the path as we can "see" within the corridor space.
        /// </remarks>
        /// <param name="path">Path to straighten.</param>
        /// <returns>Straightened path.</returns>
        private List<Point2D<int>> Straighten(List<Point2D<int>> path)
        {
            if (path.Count > 0)
            {
                List<Point2D<int>> straightened = new List<Point2D<int>>();
                int lastX = 0;
                int lastY = 0;
                var current = path[0];
                double x = current.X;
                double y = current.Y;
                int xi = (int)(x + 0.5);
                int yi = (int)(y + 0.5);
                double heading = 0;

                int bail = path.Count() * 2;
                int low = 0;
                var goal = path.Last();
                while ((xi != goal.X || yi != goal.Y) && --bail > 0)
                {
                    Point2D<int> cur = new Point2D<int>((int)(x + 0.5), (int)(y + 0.5));

                    // binary search for "steps ahead" value that isn't occluded
                    int high = path.Count - 1;
                    int point = 0;
                    Point2D<int> target = new Point2D<int>();
                    double distToTarg = 0;
                    while (high >= low)
                    {
                        point = (high - low) / 2 + low;
                        target = path[point];
                        heading = Math.Atan2((target.X - cur.X), (target.Y - cur.Y));
                        var distToObs = this.DistanceToBoundaryOfCorridor((int)x, (int)y, heading, 2);
                        distToTarg = EuclideanDistance(cur, target);

                        if (distToTarg < distToObs)
                        {
                            low = point + 1;
                        }
                        else
                        {
                            high = point - 1;
                        }
                    }

                    low = point;

                    x += Math.Sin(heading);
                    y += Math.Cos(heading);
                    xi = (int)(x + 0.5);
                    yi = (int)(y + 0.5);
                    if (xi != lastX || yi != lastY)
                    {
                        lastX = xi;
                        lastY = yi;
                        var i = xi + yi * this.mapWidth;
                        straightened.Add(new Point2D<int>(xi, yi));
                    }
                }

                return straightened;
            }

            return path;
        }
    }
}
